#ifndef QUEEN_H
#define QUEEN_H

#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include "StateMachine.h"
#include "findingPath.h"
#include "PositionInformation.h"

using std::cout;
using std::endl;
using std::queue;

string MAP_PATH = "actual_map.txt";
string TAG_PATH_1 = "tags_phase_1.txt";

struct dist_node
{
  vec2D pos;
  int dist;
};

void handle_phase_1();
void handle_phase_2();
void state_update(const std_msgs::Int32::ConstPtr& state);
void readMap(string path);
int calculate_distance(vec2D start, vec2D end);
vector<vector<int> > calculate_distance_matrix();
void load_tags(string path);

struct Tag
{
  int id;
  int shape;
  int colour;
  
  vec2D position;
  
  int heading;
  
  float prob_shape;
  float prob_colour;
};


#endif
